perm filename TCALC.OLD[AL,HE] blob
sn#501006 filedate 1980-03-24 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00021 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00003 00002 IFCR ¬DECLARATION(EXTENDED_COMPILATION)
C00004 00003 ! Load module requirements, declarations
C00006 00004 ! SPLANVAL, VPLANVAL, PPLANVAL
C00009 00005 ! Small utilities: PLACESOL, DEVBITS
C00015 00006 ! TRJCLC
C00025 00007 ! Segment time calculators: DEPTIME, RUNTIME
C00031 00008 ! Matrix solvers: DECOMPOSE, SOLVE
C00037 00009 ! POLY, the polynomial spliner: The A matrix
C00044 00010 ! POLY continued: The B vectors
C00048 00011 ! Main body of TRJCLC starts here
C00051 00012 ! Initialize the first node of the motion
C00055 00013 ! Put intermediate points into the thread
C00061 00014 ! Treat the approach
C00068 00015 ! Check for overall time constraints. Fulfil them if possible
C00073 00016 ! Call the polynomial generator on chunks of the motion.
C00079 00017 ! Compute the gravity and inertia terms
C00081 00018 ! Output the motion table
C00092 00019 ! Reclaim all the arrays in the motion thread
C00093 00020 ! CENTCLC, STOPCLC
C00096 00021 ! Bugs
C00097 ENDMK
C⊗;
IFCR ¬DECLARATION(EXTENDED_COMPILATION)
THENC
ENTRY;
BEGIN "tcalc"
IFCR ¬DECLARATION(CREFFING) THENC DEFINE CREFFING = "FALSE"; ENDC
IFCR ¬ CREFFING THENC
COMMENT: Source file requirements;
REQUIRE "ABBREV.SAI[AL,HE]" SOURCE_FILE;
REQUIRE "MACROS.SAI[AL,HE]" SOURCE_FILE;
REQUIRE "RECAUX.HDR[AL,HE]" SOURCE_FILE;
REQUIRE "ARITH.HDR[AL,HE]" SOURCE_FILE ;
REQUIRE "ALREC.SAI[AL,HE]" SOURCE_FILE ;
REQUIRE "INTDEF.SAI[AL,HE]" SOURCE_FILE;
ENDC
REQUIRE "EMITER.HDR[AL,HE]" SOURCE_FILE;
REDEFINE $$PRGID "[]" = ["TCALC"];
ENDC
! Load module requirements, declarations;
REQUIRE "ARMSOL.REL[AL,HE]" LOAD_MODULE;
EXTERNAL SAFE REAL ARRAY LOSTOP, HISTOP, TIMFAC[0:1,1:7];
! First word of array [1:14]. LOSTOP and HISTOP are joint limits,
and TIMFAC is the time in jiffies needed to move one degree (or
inch);
EXTERNAL INTEGER PROCEDURE ARMSOL(REAL ARRAY ANGLE;
RPTR(FRAME,TRANS,SVAL) PTR;INTEGER MECH);
DEFINE DEBUG = "FALSE";
DEFINE YARM_MECH = "'1";
DEFINE YHAND_MECH = "'2";
DEFINE BARM_MECH = "'4";
DEFINE BHAND_MECH = "'10";
DEFINE AHAND_MECH = "'12";
DEFINE ANARM_MECH = "'5";
DEFINE VISE_MECH = "'20";
DEFINE DRIVER_MECH = "'40";
DEFINE YARMSB = "'176000";
DEFINE YHANDSB = "'1000";
DEFINE BARMSB = "'770";
DEFINE BHANDSB = "'4";
DEFINE VISESB = "'2";
DEFINE DRIVERSB = "'1";
! SPLANVAL, VPLANVAL, PPLANVAL;
RPTR(SVAL) PROCEDURE SPLANVAL(RANY OFTHIS);
BEGIN "splanval"
! Returns a scalar as the planning value of this expression;
IF RECTYPE(OFTHIS)=LOC(SVAL)
THEN RETURN(OFTHIS)
ELSE IF RECTYPE(OFTHIS)=LOC(DEXPR)
THEN RETURN(DEXPR:VAL[OFTHIS])
ELSE COMERR("SPLANVAL garbage",OFTHIS);
END "splanval";
RPTR(V3ECT) PROCEDURE VPLANVAL(RANY OFTHIS);
BEGIN "vplanval"
! Returns a vector as the planning value of this expression;
IF RECTYPE(OFTHIS)=LOC(V3ECT)
THEN RETURN(OFTHIS)
ELSE IF RECTYPE(OFTHIS)=LOC(DEXPR)
THEN RETURN(DEXPR:VAL[OFTHIS])
ELSE COMERR("VPLANVAL garbage",OFTHIS);
END "vplanval";
RPTR(VALU$) PROCEDURE PPLANVAL (RANY OFTHIS; REFERENCE BOOLEAN SUCCESS);
BEGIN "pplanval"
! Returns a sval or frame as the planning value of this place expression;
SUCCESS ← TRUE;
IF RECTYPE(OFTHIS)=LOC(SVAL)
THEN RETURN(OFTHIS)
ELSE IF RECTYPE(OFTHIS)=LOC(FRAME)
THEN RETURN(OFTHIS)
ELSE IF RECTYPE(OFTHIS)=LOC(TRANS)
THEN RETURN(OFTHIS)
ELSE IF RECTYPE(OFTHIS)=LOC(DEXPR)
THEN RETURN(DEXPR:VAL[OFTHIS])
ELSE SUCCESS ← FALSE;
RETURN(BPARK); ! The default, to prevent more error messages;
END "pplanval";
! Small utilities: PLACESOL, DEVBITS;
SAFE OWN REAL ARRAY YELLOW,BLUE[1:6];
SIMPLE PROCEDURE INI_POS; ! initialize arm positions;
BEGIN EXTERNAL REAL ARRAY PARK[0:1,1:6];
ARRBLT(YELLOW[1],PARK[0,1],6);ARRBLT(BLUE[1],PARK[1,1],6) END;
REQUIRE INI_POS INITIALIZATION;
PROCEDURE PLACESOL
(REAL ARRAY RES; RPTR(VALU$) PTR; INTEGER MECH; REFERENCE INTEGER FLG);
! PTR points to a frame/trans or a scalar constant.
Its solution is calculated in RES. If armsol has trouble with the
location, then FLG is set TRUE. On the other hand, if it is a
scalar, HANDSOL is called to check bounds (setting FLG) and
to store the result into RES. In any case, MECH specifies
which mechanism (eg BARM) is meant.
;
BEGIN "plcslv"
INTEGER LOJOINT;
LOJOINT ← ARRINFO(RES,1);
IF MECH LAND AHAND_MECH THEN
BEGIN "handsol"
FLG ← ARMSOL(RES,PTR,MECH);
RETURN
END "handsol";
IF MECH = YARM_MECH THEN
BEGIN "yarm"
FLG ← ARMSOL(YELLOW,PTR,YARM_MECH);
ARRBLT(RES[LOJOINT],YELLOW[1],6);
END
ELSE IF MECH = BARM_MECH THEN
BEGIN "barm"
FLG ← ARMSOL(BLUE,PTR,BARM_MECH);
ARRBLT(RES[LOJOINT],BLUE[1],6);
END
ELSE USERERR(0,1,"PLACESOL: unknown device "&CVOS(MECH));
IF FLG THEN PRINT(CRLF & "Joints out of range: ",CVOS(FLG),CRLF);
END "plcslv";
PROCEDURE DEVBITS (REFERENCE INTEGER ARM, SBITS, LOJOINT, HIJOINT; RVAR WHAT);
BEGIN "devbits";
! Takes WHAT as a device name and fills in ARM, SBITS, LOJOINT,
HIJOINT;
IF WHAT = YARM
THEN BEGIN ! Yellow arm;
LOJOINT ← 1;
HIJOINT ← 6;
ARM ← YARM_MECH;
SBITS ← YARMSB;
END
ELSE IF WHAT = YHAND
THEN BEGIN ! Yellow hand;
LOJOINT ← 7;
HIJOINT ← 7;
ARM ← YHAND_MECH;
SBITS ← YHANDSB;
END
ELSE IF WHAT = BARM
THEN BEGIN ! Blue arm;
LOJOINT ← 8;
HIJOINT ← 13;
ARM ← BARM_MECH;
SBITS ← BARMSB;
END
ELSE IF WHAT = BHAND
THEN BEGIN ! Blue hand;
LOJOINT ← 14;
HIJOINT ← 14;
ARM ← BHAND_MECH;
SBITS ← BHANDSB;
END
ELSE IF WHAT = VISE
THEN BEGIN ! Vise;
LOJOINT ← 15;
HIJOINT ← 15;
ARM ← VISE_MECH;
SBITS ← VISESB;
END
ELSE IF WHAT = DRIVER
THEN BEGIN ! Driver;
LOJOINT ← 16;
HIJOINT ← 16;
ARM ← DRIVER_MECH;
SBITS ← DRIVERSB;
END
ELSE BEGIN ! Wrong arm;
COMERR("DEVBITS: No such arm; assuming BLUE.");
LOJOINT ← 8;
HIJOINT ← 13;
ARM ← BARM_MECH;
SBITS ← BARMSB;
END;
END "devbits";
! TRJCLC;
INTERNAL PROCEDURE TRJCLC (RPTR(MOVE$) MOV);
BEGIN "trjclc"
RCLASS TTHREAD (
REAL STIME, UTIME; INTEGER MODE;
RPTR(VARIABLE) EVENT;
RPTR(VARIABLE,VALU$,DEXPR) PLACE;
REAL ARRAY ANGLES, VELS; ! [LOJOINT:HIJOINT];
REAL ARRAY COEFF; ! [1:6,0:5]=[joint,degree] polynomial coefficients;
REAL ARRAY GRAVIN; ! [1:12] gravity, inertia terms for each joint;
RPTR(TTHREAD) NEXT
);
DEFINE TIME_MODE = '3;
DEFINE DEPA_MODE = '4;
DEFINE APPR_MODE = '10;
DEFINE ENDP_MODE = '20;
DEFINE INVI_MODE = '40;
!
Data structures:
A TTHREAD is a linked list of points along which the trajectory
passes. It has these fields:
MODE(INTEGER)
The TIME_MODE bits relate to UTIME:
0:no bound, 1:lower bound, 2:upper bound, 3:exact bound.
DEPA_MODE: on if this point is a departure
APPR_MODE: on if this point is an approach
ENDP_MODE: on if this point is an endpoint (either one)
INVI_MODE: on if this point ends a segment whose time is
inviolate. This applies to the endpoint segments only.
STIME(REAL)
System-calculated time in seconds since previous node. If
there is ia conflict between user and system, then the
resolved time is placed in STIME. That causes problems: The
system time is destroyed, so global resolutions use whatever
foolish thing the user wanted.
UTIME(REAL)
User-supplied time in seconds since previous node.
PLACE(RVAR)
Variable (eventually expression) which has the location of
the mechanism as this node is achieved. This can refer to an
arm (in which case the variable will be a frame) or a hand
(in which case it will be a scalar).
ANGLES(n-VECTOR)
Joint angles for this node, if there is an associated place.
VELS(6-VECTOR)
Joint velocities for this node (deg/sec) if there are some.
EVENT(VARIABLE)
Event to signal to start up code when this node reached.
COEFF(6x6 MATRIX)
The 6 coefficients of the segment ending at this node.
NEXT(PTR(TTHREAD))
Next node.
The trajectory calculator turns motion specifications into
interpretable tables. At the moment it allows any one mechanism,
that is, one arm or one hand. Future work will allow any
combination of mechanisms. The tables are calculated by the
following method:
A thread is made, with a node for each place in the motion
specification, that is, the initial point, the departure, if any,
the via points, the approach point, and the destination. Arm or
hand solutions are calculated for each node. It may be that this
serial calculation will lead to flips of the arm. If this
happens, the proper order is outside-in. This is because the
ARMSOL routine uses the previous solution to resolve ambiguities
in joint 4 of the Scheinman arms.
Any deproach points or calculated via points or calculated
destinations must have code emitted to make a cell for them in
the graph structure. The cell for a departure is marked
permanently invalid. Its calculator uses the hand position
itself, not the place where the arm was to be at the start of the
motion. The cells for the calculated via points and the approach
point are in the graph structure in the usual way. This code
must be emitted at the outermost practical point in the program:
If it is too far in, then it gets redone too often, and if it is
too far out, it might cause graph structure to hang around
associated to non-existent nodes. In any case, it is necessary
to put such code at a block entry, and to be sure to get rid of
the resulting graph structure at block exit. The current code
does not handle any of this.
At this time, the fourth degree polynomials for deproach segments
are calculated, and any given velocity constraints are noted.
The presence of a velocity constraint implies that the
acceleration is constrained to zero. If the user has supplied a
time, it is put in UTIME, and STIME is computed by the system.
If they are compatible, STIME is modified to the final decision
on the time for the segment.
After the entire thread is made, a global check is made to
insure that the timing is in agreement with the user's wishes.
Then the thread is divided into chunks, where each chunk is the
region between two velocity-constrained points (the deproach
points are such). A chunk which has only two points (but not a
deproach chunk, for which the trajectory has already been
calculated) gets a fifth-degree polynomial calculated to match
all the constraints. A chunk with more points requires splining
for the trajectory. The first step is to insert one
unconstrained point in each of the two longest intervals. It has
been found that the best place for these points is almost at one
end of the intervals (.001 of the way to the end) to
minimise overshoot problems. After the fully unconstrained nodes
have been inserted into the thread, the routine POLY is called to
create the coefficients of the third degree splined polynomial.
It has been found that using fourth degree polynomials in two of
the segments instead of inserting two unconstrained points leads
to uncontrollable overshoot. Finally, the resulting trajectory
is emitted.
The following conventions are used for arms and joints. Joints
1-6 are yellow arm (arm 0), and joint 7 is the yellow fingers
(arm 2). Joints 8-13 are the blue arm (arm 1), and joint 14 is
the blue fingers (arm 3). The angle arrays are tailored to have
whatever joints are needed. The arm and hand solution programs
are told which mechanism to expect.
The current code does not check location, velocity or
acceleration bounds except for location bounds at user-specified
places. Instead, location bounds are to a large extent insured
by the servo. Velocity and acceleration can be optimized by
rescaling time, in the cases when the user has not specified any
time in the entire motion, nor any velocities, but this is not
currently attempted.
;
! Segment time calculators: DEPTIME, RUNTIME;
INTERNAL INTEGER SPEED; ! If > 1, motions are slower;
REAL PROCEDURE DEPTIME(REAL ARRAY ENDANG, DEPANG, DEL);
BEGIN "deptim"
! Uses the endpoint and deproach point joint angles and
determines the correct time for the deproach between them,
which it returns. If the deproach point is more than half
the way from the endpoint to the joint limit, it is moved to
the halfway point. This means that DEPANG CAN BE CHANGED BY
THIS PROCEDURE!! This is to prevent deproaches which
overstrain the arm. The resulting DEPANG-ENDANG is stored in
DEL.
;
INTEGER JOINT, ! For loop control;
LOJOINT, HIJOINT; ! Defines which arm;
REAL DELTA, ! For joint angle differences;
TAU, TTAU, ! For times;
TEMP;
LOJOINT ← ARRINFO(ENDANG,1);
HIJOINT ← ARRINFO(ENDANG,2);
! If the distance from ENDANG to DEPANG is more than half
the distance to the joint limit, then modify DEPANG to fall
within such a restriction;
FOR JOINT ← LOJOINT STEP 1 UNTIL HIJOINT DO
BEGIN ! Each iteration checks one joint for stop violation;
DEL[JOINT] ← DELTA ← DEPANG[JOINT] - ENDANG[JOINT];
IF (TEMP ← MEMORY[LOC(HISTOP[0,1])-1+JOINT,REAL]-ENDANG[JOINT]) < 2*DELTA
THEN BEGIN ! Exceed high stop;
DEPANG[JOINT] ← (MEMORY[LOC(HISTOP[0,1])-1+JOINT,REAL]+ENDANG[JOINT])/2.;
DEL[JOINT] ← TEMP / 2.;
END
ELSE IF
(TEMP ← MEMORY[LOC(LOSTOP[0,1])-1+JOINT,REAL]-ENDANG[JOINT]) > 2*DELTA
THEN BEGIN ! Exceed low stop;
DEPANG[JOINT] ← (MEMORY[LOC(LOSTOP[0,1])-1+JOINT,REAL]+ENDANG[JOINT])/2.;
DEL[JOINT] ← TEMP / 2.;
END;
END;
TAU ← TTAU ← 0.;
! Find maximum required time for the motion;
FOR JOINT ← LOJOINT STEP 1 UNTIL HIJOINT DO
BEGIN
TTAU ← MEMORY[LOC(TIMFAC[0,1])-1+JOINT,REAL] * ABS(DEL[JOINT]);
IF TTAU ≥ TAU THEN TAU ← TTAU;
END;
! If you want to use LOU's model of linear accleration,
you should insert here:
IF (TEMP←.4*SQRT(distance moved in cm.)) > TAU THEN TAU ← TEMP;
! This .4 (sec*sqrt(cm)) is based on experience (LOU
1/13/75), it accounts for the distance moved according
to a linear acceleration model;
TAU ← SPEED * TAU; ! Account for speed_factor specification;
RETURN (TAU/60. + .3); ! Add on some slack time,
convert jiffies → seconds;
END "deptim";
REAL PROCEDURE RUNTIME(REAL ARRAY OLDANG, NEWANG);
BEGIN "runtim"
! Uses the old and the new joint angles to determine the
correct time in seconds for one segment of motion;
INTEGER JOINT, ! For loop control;
LOJOINT, HIJOINT; ! Defines which arm;
REAL TAU, TTAU;
TAU ← TTAU ← 0;
LOJOINT ← ARRINFO(OLDANG,1);
HIJOINT ← ARRINFO(OLDANG,2);
FOR JOINT ← LOJOINT STEP 1 UNTIL HIJOINT DO
BEGIN
TTAU ← MEMORY[LOC(TIMFAC[0,1])-1+JOINT,REAL] *
ABS(OLDANG[JOINT]-NEWANG[JOINT]);
IF TTAU ≥ TAU THEN TAU ← TTAU;
END;
TAU ← SPEED * TAU; ! Account for speed_factor specification;
! Add on some slack time and convert jiffies → seconds;
IF LOJOINT=HIJOINT ! hand;
THEN RETURN(TAU/60.0 + 0.3) ! Opening fingers;
! IF NEWANG[LOJOINT] > OLDANG[LOJOINT] ∧ SPEED < 2 ! opening or closing?;
! ELSE RETURN(TAU/60.0 + 0.2) ! Closing fingers is faster;
ELSE RETURN (TAU/60.0 + 0.2);
! RETURN ((TAU/60.0 + 0.4) MAX .6); ! Used to be just this;
END "runtim";
! Matrix solvers: DECOMPOSE, SOLVE;
SAFE OWN INTEGER ARRAY PS[1:50];
PROCEDURE DECOMPOSE(INTEGER N;SAFE REAL ARRAY A,LU);
! Both A and LU are [1:N, 1:N]. Uses global array PS.
Computes triangular matrices L and U and permutation matrix
PS so that LU=PA. Stores (L-I) and U both in LU. The call
DECOMPOSE(N,A,A) will overwrite A with LU.
;
BEGIN "decompose"
INTEGER I, J, K, PIVOTINDEX;
REAL NORMROW, PIVOT, SIZE, BIGGEST, MULT;
SAFE OWN REAL ARRAY R[1:50];
SIMPLE PROCEDURE ILOOP(INTEGER UL;REFERENCE REAL R1,R2);
! Machine-coded for efficiency;
START_CODE
LABEL LP,EU;
MOVE 1,-1('17);
MOVE 2,-2('17);
MOVE 3,-3('17);
SUB 3,K;
JUMPLE 3,EU;
LP: AOJ 1,;
AOJ 2,;
MOVN 4,MULT;
FMPR 4,(1);
FADRM 4,(2);
SOJG 3,LP;
EU: END;
IF N > 50
THEN COMERR("DECOMPOSE can't handle a matrix as large as" & CVS(N),RNULL);
! Initialize PS,LU and R;
FOR I←1 STEP 1 UNTIL N DO
BEGIN
PS[I]←I;
NORMROW←0;
FOR J←1 STEP 1 UNTIL N DO
BEGIN
LU[I,J]←A[I,J];
IF (NORMROW<ABS(LU[I,J])) THEN NORMROW←ABS(LU[I,J]);
END;
IF (NORMROW≠0)
THEN R[I]←1/NORMROW
ELSE BEGIN
R[I]←0;
COMERR("Zero row in DECOMPOSE",RNULL);
END;
END;
! Gaussian elimination with partial pivoting;
FOR K←1 STEP 1 UNTIL N-1 DO
BEGIN "kloop";
BIGGEST ← 0;
FOR I ← K STEP 1 UNTIL N DO
BEGIN
SIZE←ABS(LU[PS[I],K])*R[PS[I]];
IF (BIGGEST<SIZE)
THEN BEGIN
BIGGEST←SIZE;
PIVOTINDEX←I;
END;
END;
IF BIGGEST = 0
THEN BEGIN
COMERR("Singular matrix in DECOMPOSE",RNULL);
DONE "kloop";
END;
IF PIVOTINDEX ≠ K
THEN BEGIN
J←PS[K];
PS[K]←PS[PIVOTINDEX];
PS[PIVOTINDEX]←J;
END;
PIVOT←LU[PS[K],K];
FOR I←K+1 STEP 1 UNTIL N DO
BEGIN
LU[PS[I],K]←MULT←(LU[PS[I],K]/PIVOT);
IF MULT ≠ 0
THEN ILOOP(N,LU[PS[I],K],LU[PS[K],K]);
! The following is the result of the machine code:
FOR J ← K+1 STEP 1 UNTIL N DO
LU[PS[I],J]←LU[PS[I],J]-MULT*LU[PS[K],J];
END;
END "kloop";
IF (LU[PS[N],N]=0)
THEN COMERR("Singular matrix in DECOMPOSE",RNULL);
END "decompose";
SIMPLE PROCEDURE SOLVE(INTEGER N;SAFE REAL ARRAY LU,B,X);
! Arrays LU[1:N,1:N], B,X[1:N]. Uses global safe integer
array PS. Solves AX=B using LU from DECOMPOSE.
;
BEGIN "solve"
INTEGER I,J;
REAL DOT;
SIMPLE PROCEDURE ILOOP(INTEGER LL,UL;REFERENCE REAL R1,R2);
! Machine-coded for efficiency;
START_CODE
LABEL LP,EU;
MOVE 1,-1('17);
MOVE 2,-2('17);
MOVE 3,-3('17);
SUB 3,-4('17);
SETZ 4,;
JUMPL 3,EU;
LP: MOVE 5,(1);
FMPR 5,(2);
FADR 4,5;
AOJ 1,;
AOJ 2,;
SOJGE 3,LP;
EU: MOVEM 4,DOT;
END;
FOR I ← 1 STEP 1 UNTIL N DO
BEGIN
ILOOP(1,I-1,LU[PS[I],1],X[1]);
! Has this effect:
DOT←0
FOR J←1 STEP 1 UNTIL I-1 DO
DOT←DOT+LU[PS[I],J]*X[J];
X[I]←B[PS[I]]-DOT;
END;
X[N] ← X[N] / LU[PS[N],N];
FOR I ← N-1 STEP -1 UNTIL 1 DO
BEGIN ! RF: I changed loop upper index from N, to avoid
subscript errors;
ILOOP(I+1,N,LU[PS[I],I+1],X[I+1]);
! Has this effect:
DOT←0
FOR J←I+1 STEP 1 UNTIL N DO
DOT←DOT+LU[PS[I],J]*X[J];
X[I]←(X[I]-DOT)/LU[PS[I],I];
END;
END "solve";
! POLY, the polynomial spliner: The A matrix;
PROCEDURE POLY (RPTR(TTHREAD) FIRST, LAST; INTEGER LOJ, HIJ, NS);
! Calculate a trajectory for joints LOJ through HIJ using
the thread from FIRST to LAST. The number of segments in the
chunk is given by NS. The location for each node is to be
found in TTHREAD:ANGLES[*][JOINT], except for the
unconstrained points, which are distinguishable in that
TTHREAD:PLACE[*] = RNULL. The velocities of the first and
last points are given in TTHREAD:VELS[*][JOINT]. It is assumed
that the accelerations at these points are to be zero. The
timing for each segment is found in TTHREAD:STIME[*] in the
node at the end of the segment. The coefficients of the
resulting polynomial will be stored in the thread nodes, as
TTHREAD:COEFFS[*][JOINT,degree]. ;
BEGIN "poly"
DEFINE MEM(ARG) "<>" = <MEMORY[ARG,REAL]>;
SAFE REAL ARRAY A [1:4*NS,1:4*NS];
SAFE REAL ARRAY B, X [1:4*NS];
! A is a large matrix and B is a vector, for which we will
solve AX=B. A is the same for each joint, but B is
calculated anew for each joint. Thus only one call to
DECOMPOSE is needed;
RPTR(TTHREAD) P, Q; ! Used in tracking down the motion thread;
INTEGER ROW, COL, N, SEG, ALOC, I, JOINT; ! ALOC is used to point into A;
REAL TEMP;
ARRCLR(A);
N ← 4 * NS;
ROW ← COL ← 1;
! Compute the A matrix decomposition:;
! Fill the A matrix for the first segment;
ALOC ← LOCATION(A[1,1]);
A[ROW,COL] ← A[ROW+1,COL+1] ← 1.;
A[ROW+2,COL+2] ← 2.;
ROW ← ROW + 3;
COL ← COL + 4;
ALOC ← ALOC + 3*N + 4;
Q ← TTHREAD:NEXT[FIRST];
P ← TTHREAD:NEXT[Q];
FOR SEG ← 2 STEP 1 UNTIL NS DO
BEGIN "asegpol"
! Look at segment twixt Q and P;
IF TTHREAD:PLACE[Q] = RNULL
THEN BEGIN "auncnst" ! Left of segment is unconstrained;
MEM(ALOC) ← 1.;
MEM(ALOC-4) ← MEM(ALOC-3) ← MEM(ALOC-2) ← MEM(ALOC-1) ←
-1.;
ALOC ← ALOC + N;
MEM(ALOC-3) ← -1.;
MEM(ALOC-2) ← -2.;
MEM(ALOC-1) ← -3.;
MEM(ALOC+1) ← TEMP ← TTHREAD:STIME[Q]/TTHREAD:STIME[P];
ALOC ← ALOC + N;
MEM(ALOC-2) ← -1.;
MEM(ALOC-1) ← -3.;
MEM(ALOC+2) ← TEMP*TEMP;
! Same effect as:
A[ROW,COL] ← 1.
A[ROW,COL-1] ← A[ROW,COL-2] ← A[ROW,COL-3]
← A[ROW,COL-4] ← A[ROW+1,COL-3]
← A[ROW+2,COL-2] ← -1.
A[ROW+1,COL-1] ← A[ROW+2,COL-1] ← -3.
A[ROW+1,COL-2] ← -2.
A[ROW+2,COL+2] ← (A[ROW+1,COL+1] ←
TTHREAD:STIME[Q]/TTHREAD:STIME[P]) ↑ 2;
ROW ← ROW + 3;
COL ← COL + 4;
ALOC ← ALOC + N + 4;
END "auncnst"
ELSE BEGIN "acnst" ! Left of segment is constrained;
MEM(ALOC-1) ← MEM(ALOC-2) ← MEM(ALOC-3) ← MEM(ALOC-4)
← MEM(ALOC+N) ← 1.;
ALOC ← ALOC + N + N;
MEM(ALOC-1) ← -3.;
MEM(ALOC-2) ← -2.;
MEM(ALOC-3) ← -1.;
MEM(ALOC+1) ← TEMP ← TTHREAD:STIME[Q] / TTHREAD:STIME[P];
ALOC ← ALOC + N;
MEM(ALOC-2) ← -1.;
MEM(ALOC-1) ← -3.;
MEM(ALOC+2) ← TEMP*TEMP;
! Equivalent to:
A[ROW,COL-1] ← A[ROW,COL-2]
← A[ROW,COL-3] ← A[ROW,COL-4] ← A[ROW+1,COL] ← 1.
A[ROW+2,COL-3] ← A[ROW+3,COL-2] ← -1.
A[ROW+2,COL-1] ← A[ROW+3,COL-1] ← -3.
A[ROW+2,COL-2] ← -2.
A[ROW+3,COL+2] ← (A[ROW+2,COL+1] ←
TTHREAD:STIME[Q]/TTHREAD:STIME[P]) ↑ 2;
ROW ← ROW + 4;
COL ← COL + 4;
ALOC ← ALOC + N + 4;
END "acnst";
Q ← P;
P ← TTHREAD:NEXT[Q];
END "asegpol";
! Take care of the constraints at the final point;
COL ← COL - 4;
MEM(ALOC-4) ← MEM(ALOC-3) ← MEM(ALOC-2) ← MEM(ALOC-1) ← 1.;
ALOC ← ALOC + N;
MEM(ALOC-3) ← 1.;
MEM(ALOC-2) ← 2.;
MEM(ALOC-1) ← 3.;
ALOC ← ALOC + N;
MEM(ALOC-2) ← 2.;
MEM(ALOC-1) ← 6.;
! Equivalent to:
A[ROW,COL] ← A[ROW,COL+1] ← A[ROW,COL+2] ← A[ROW,COL+3]
← A[ROW+1,COL+1] ← 1.
A[ROW+1,COL+2] ← A[ROW+2,COL+2] ← 2.
A[ROW+1,COL+3] ← 3.
A[ROW+2,COL+3] ← 6.;
ROW ← ROW + 3;
COL ← COL + 4;
IF ROW ≠ COL ∨ ROW ≠ N + 1 THEN COMERR("ERROR IN POLY");
IF DEBUG ! Debug is defined false. Use RAID to remove the
jump around this code if you want to see the matrices;
THEN BEGIN "adebug" ! Print out the matrix A;
INTEGER WIDTH, DIGITS;
OUTSTR(CRLF);
GETFORMAT(WIDTH,DIGITS);
SETFORMAT(5,2);
FOR ROW ← 1 STEP 1 UNTIL N DO
BEGIN
FOR COL ← 1 STEP 1 UNTIL N DO
OUTSTR(CVF(A[ROW,COL]));
OUTSTR(CRLF);
END;
OUTSTR(CRLF);
SETFORMAT(WIDTH,DIGITS);
END "adebug";
DECOMPOSE(N,A,A);
! POLY continued: The B vectors;
! For each joint, calculate B, solve X and stow away;
FOR JOINT ← LOJ STEP 1 UNTIL HIJ DO
BEGIN "bcalc"
ARRCLR(B);
ROW ← 1;
! Fill the B matrix for the first segment;
B[ROW] ← TTHREAD:ANGLES[FIRST][JOINT];
B[ROW+1] ← TTHREAD:STIME[FIRST] * TTHREAD:VELS[FIRST][JOINT];
! If we ever put in non-zero acceleration constraints:
B[ROW+2] ← TTHREAD:STIME[FIRST][JOINT]↑2 * TTHREAD:ACCS[FIRST][JOINT];
ROW ← ROW + 3;
Q ← TTHREAD:NEXT[FIRST];
P ← TTHREAD:NEXT[Q];
FOR SEG ← 2 STEP 1 UNTIL NS DO
BEGIN "bsegpol"
! Look at segment twixt Q and P;
IF TTHREAD:PLACE[Q] = RNULL
THEN BEGIN "buncnst" ! Left of segment is unconstrained;
ROW ← ROW + 3;
END "buncnst"
ELSE BEGIN "bcnst" ! Left of segment is constrained;
B[ROW] ← B[ROW+1] ← TTHREAD:ANGLES[Q][JOINT];
ROW ← ROW + 4;
END "bcnst";
Q ← P;
P ← TTHREAD:NEXT[Q];
END "bsegpol";
! Take care of the constraints at the final point;
B[ROW] ← TTHREAD:ANGLES[LAST][JOINT];
B[ROW+1] ← TTHREAD:STIME[LAST] * TTHREAD:VELS[LAST][JOINT];
! If we ever put in non-zero acceleration constraints:
B[ROW+2] ← TTHREAD:STIME[LAST]↑2 * TTHREAD:ACCS[LAST][JOINT];
ROW ← ROW + 3;
IF ROW ≠ N + 1 THEN COMERR("ERROR IN POLY");
IF DEBUG ! Debug is defined false. Use RAID to remove the
jump around this code if you want to see the matrices;
THEN BEGIN "bdebug" ! Print out the matrix B;
INTEGER WIDTH, DIGITS;
OUTSTR(CRLF);
GETFORMAT(WIDTH,DIGITS);
SETFORMAT(5,2);
OUTSTR(CRLF);
FOR ROW ← 1 STEP 1 UNTIL N DO
OUTSTR(CVF(B[ROW]));
OUTSTR(CRLF);
SETFORMAT(WIDTH,DIGITS);
END "bdebug";
SOLVE(N,A,B,X);
! Stow away the answer into the coefficient matrices;
P ← TTHREAD:NEXT[FIRST];
I ← 1;
FOR SEG ← 1 STEP 1 UNTIL NS DO
BEGIN "stow" ! Each iteration stores the coefficients into one node;
ARRBLT(TTHREAD:COEFF[P][JOINT,0],X[I],4);
I ← I + 4;
P ← TTHREAD:NEXT[P];
END "stow";
END "bcalc";
END "poly";
! Main body of TRJCLC starts here;
RPTR(TTHREAD)
MOTION, ! The entire motion will be stored on this thread;
LEADTHREAD, ! A forward pointer used in scanning down thread;
CURTHREAD, OLDTHREAD; ! Used to trace down the motion;
REAL ARRAY DEL [1:14]; ! Joint angle differences;
RANY P, Q, ! Used in tracking down cell links;
TMPPLACE; ! Used as a temp in location calculations;
RPTR(VARIABLE) VAR;
REAL UT, ST, TT; ! User-defined time, system-computed time, total time;
INTEGER M, ! Holds modes;
FLAG, ! Boolean, for success parameters;
JOINT, ! For loop control;
I, ! For loop control;
LOJOINT, HIJOINT, ! Defines which arm;
ARM, ! Mechanism bits for the device used, eg YARM_MECH;
SBITS, ! Status bits for the device used, eg YARMSB;
SEGCNT, ! How many segments to the motion;
LAB, ! For code emission: a label;
DPTR, SEGLEN; ! For code emission: pointers into DATA and RELOC;
INTEGER ARRAY DATA, RELOC [0:1000]; ! Used for emitting code;
DEVBITS(ARM,SBITS,LOJOINT,HIJOINT,MOVE$:CF[MOV]);
SEGCNT ← 0;
TT ← 0.;
! Establish the speed factor for this move;
SPEED ← MOVE$:SFAC[MOV]; ! Bound by WLDMOD;
IF SPEED < 1 THEN
BEGIN "too fast"
COMERR("Speed_factor for MOVE < 1, setting it to 1",MOV);
SPEED ← 1;
END "too fast";
! Initialize the first node of the motion;
MOTION ← NEW_RECORD(TTHREAD);
TTHREAD:PLACE[MOTION] ← MOVE$:CFVAL[MOV]; ! active arm's current frame;
TTHREAD:MODE[MOTION] ← ENDP_MODE; ! endpoint;
NewArray(REAL,TTHREAD:ANGLES[MOTION],[LOJOINT:HIJOINT]);
NewArray(REAL,TTHREAD:VELS[MOTION],[LOJOINT:HIJOINT]);
TMPPLACE ← PPLANVAL(TTHREAD:PLACE[MOTION],FLAG);
IF ¬FLAG THEN COMERR("Illegal start point",MOV);
PLACESOL(TTHREAD:ANGLES[MOTION],TMPPLACE,ARM,FLAG);
IF FLAG THEN
COMERR("The initial location is not accessible.
The closest reasonable point is being used.",MOV);
CURTHREAD ← MOTION;
! Treat the departure;
P ← MOVE$:CLAUSES[MOV];
WHILE (P≠RNULL) ∧ (RECTYPE(CELL:CAR[P])≠LOC(DEPARTURE)) DO P ← CELL:CDR[P];
IF (P ≠ RNULL) ∧ (DEPARTURE:THRU[CELL:CAR[P]] ≠ NILDEPROACH)
THEN BEGIN "depart" ! Won't work for fingers, of course;
IF LOJOINT = HIJOINT THEN COMERR("No deproaches allowed for fingers");
SEGCNT ← SEGCNT + 1;
OLDTHREAD ← CURTHREAD;
CURTHREAD ← TTHREAD:NEXT[OLDTHREAD] ← NEW_RECORD(TTHREAD);
TTHREAD:PLACE[CURTHREAD] ← DEPARTURE:ACTPLACE[CELL:CAR[P]];
NewArray(REAL,TTHREAD:ANGLES[CURTHREAD],[LOJOINT:HIJOINT]);
TMPPLACE ← PPLANVAL(TTHREAD:PLACE[CURTHREAD],FLAG);
IF ¬FLAG THEN COMERR("Illegal departure point",MOV);
PLACESOL(TTHREAD:ANGLES[CURTHREAD],TMPPLACE,ARM,FLAG);
IF FLAG THEN
COMERR("This departure location is not accessible.
The closest reasonable point is being used.",TTHREAD:PLACE[CURTHREAD]);
TTHREAD:STIME[CURTHREAD] ←
DEPTIME(TTHREAD:ANGLES[OLDTHREAD],TTHREAD:ANGLES[CURTHREAD],DEL);
TT ← TT + TTHREAD:STIME[CURTHREAD];
NewArray(REAL,TTHREAD:VELS[CURTHREAD],[LOJOINT:HIJOINT]);
NewArray(REAL,TTHREAD:COEFF[CURTHREAD],[LOJOINT:HIJOINT,0:5]);
FOR JOINT ← LOJOINT STEP 1 UNTIL HIJOINT DO
BEGIN ! stow away the coefficients for this joint.
The poly for each angle is -del*t↑4 + 2*del*t↑3 + initial;
REAL DL;
DL ← DEL[JOINT];
TTHREAD:COEFF[CURTHREAD][JOINT,4] ← -DL;
TTHREAD:COEFF[CURTHREAD][JOINT,3] ← 2. * DL;
TTHREAD:COEFF[CURTHREAD][JOINT,0] ←
TTHREAD:ANGLES[OLDTHREAD][JOINT];
TTHREAD:VELS[CURTHREAD][JOINT] ←
2. * DL / TTHREAD:STIME[CURTHREAD];
END;
TTHREAD:MODE[CURTHREAD] ← DEPA_MODE + INVI_MODE;
END "depart";
! Put intermediate points into the thread;
P ← MOVE$:CLAUSES[MOV];
WHILE TRUE DO
BEGIN "interm"
! This loop is terminated by a DONE. Each iteration looks at
the next via on the clauses list;
RPTR(VIA) VIAP;
WHILE (P≠RNULL) ∧ (RECTYPE(CELL:CAR[P])≠LOC(VIA)) DO
P ← CELL:CDR[P];
IF P = RNULL THEN DONE "interm";
VIAP ← CELL:CAR[P];
SEGCNT ← SEGCNT + 1;
OLDTHREAD ← CURTHREAD;
CURTHREAD ← TTHREAD:NEXT[OLDTHREAD] ← NEW_RECORD(TTHREAD);
IF VIA:CODE[VIAP] ≠ RNULL THEN ! Set up event to signal for code at VIA;
TTHREAD:EVENT[CURTHREAD] ←
IF RECTYPE(VIA:CODE[VIAP])=LOC(CMON)
THEN CMON:CONDITION[VIA:CODE[VIAP]]
ELSE EVDO:VAR[VIA:CODE[VIAP]];
TTHREAD:PLACE[CURTHREAD] ← VIA:ACTPLACE[VIAP];
NewArray(REAL,TTHREAD:ANGLES[CURTHREAD],[LOJOINT:HIJOINT]);
NewArray(REAL,TTHREAD:COEFF[CURTHREAD],[LOJOINT:HIJOINT,0:5]);
TMPPLACE ← PPLANVAL(TTHREAD:PLACE[CURTHREAD],FLAG);
IF ¬FLAG THEN COMERR("Illegal via point",MOV);
PLACESOL(TTHREAD:ANGLES[CURTHREAD],TMPPLACE,ARM,FLAG);
IF FLAG THEN
COMERR("This via location is not accessible.
The closest reasonable point is being used.",
CONS(MOV,CONS(TTHREAD:PLACE[CURTHREAD],RNULL)));
IF VIA:VELOC[VIAP] ≠ RNULL
THEN BEGIN ! There is a velocity specification here;
IF LOJOINT = HIJOINT
THEN TTHREAD:VELS[CURTHREAD][LOJOINT]
← SVAL:VAL[SPLANVAL(VELOCITY:VELOC[VIA:VELOC[VIAP]])]
ELSE BEGIN "fvel"
RPTR(V3ECT) VTEMP,V2TEMP; ! To hold offset vector (inches/second);
RPTR(TRANS) FTEMP; ! To hold frame value;
REAL ARRAY OFFANG [1:6]; ! Offset angles;
INTEGER I; ! Loop control;
NewArray(REAL,TTHREAD:VELS[CURTHREAD],[LOJOINT:HIJOINT]);
FTEMP ← IF RECTYPE(TMPPLACE)=LOC(FRAME) THEN FRAME:VAL[TMPPLACE]
ELSE TMPPLACE;
VTEMP ← VPLANVAL(VELOCITY:VELOC[VIA:VELOC[VIAP]]);
VTEMP ← V3ADD(TRANS:P[FTEMP],SVMUL(0.2,VTEMP)); ! Add in offset vector;
ARRBLT(OFFANG[1],TTHREAD:ANGLES[CURTHREAD][LOJOINT],6);
! So the out-of bounds result will be reasonable;
V2TEMP ← TRANS:P[FTEMP];
TRANS:P[FTEMP] ← VTEMP;
ARMSOL(OFFANG,FTEMP,IF LOJOINT=1 THEN YARM_MECH ELSE BARM_MECH);
TRANS:P[FTEMP] ← V2TEMP;
FOR JOINT ← LOJOINT STEP 1 UNTIL LOJOINT+5 DO
TTHREAD:VELS[CURTHREAD][JOINT] ←
5.*(OFFANG[JOINT-LOJOINT+1]
- TTHREAD:ANGLES[CURTHREAD][JOINT]);
END "fvel";
END;
ST ← TTHREAD:STIME[CURTHREAD] ←
RUNTIME(TTHREAD:ANGLES[OLDTHREAD],TTHREAD:ANGLES[CURTHREAD]);
IF VIA:TIME[VIAP] ≠ RNULL
THEN BEGIN ! The time is constrained;
RPTR(DURATION) DUR;
DUR ← VIA:TIME[VIAP];
UT ← TTHREAD:UTIME[CURTHREAD]
← SVAL:VAL[SPLANVAL(DURATION:TIME[DUR])];
M ← TTHREAD:MODE[CURTHREAD] ← DURATION:TIME_RELN[DUR];
! test for incompatibilites;
IF ST > UT ∧ M ≥ 2 THEN
BEGIN
COMERR(
"Cannot satisfy your time request for this segment without danger;
you want "&CVG(UT)&" seconds, and I think you need "&CVG(ST)&"
seconds. Nonetheless, I am using your request.");
IF UT ≤ 0 THEN
BEGIN
COMERR("But I refuse to let you get away with no time at all!");
UT ← ST;
END;
END;
IF (M=1 ∧ ST<UT) ∨ (M=2 ∧ ST>UT) ∨ (M=3 ∧ ST≠UT) THEN
TTHREAD:STIME[CURTHREAD] ← UT;
END;
TT ← TT + TTHREAD:STIME[CURTHREAD];
P ← CELL:CDR[P];
END "interm";
! Treat the approach;
P ← MOVE$:CLAUSES[MOV];
WHILE (P≠RNULL) ∧ (RECTYPE(CELL:CAR[P])≠LOC(APPROACH)) DO P ← CELL:CDR[P];
IF (P ≠ RNULL) ∧ (APPROACH:THRU[CELL:CAR[P]] ≠ NILDEPROACH)
THEN BEGIN "approa" ! Will not work for finger operation;
IF LOJOINT = HIJOINT THEN COMERR("No deproaches allowed for fingers");
SEGCNT ← SEGCNT + 1;
OLDTHREAD ← CURTHREAD;
CURTHREAD ← TTHREAD:NEXT[OLDTHREAD] ← NEW_RECORD(TTHREAD);
TTHREAD:PLACE[CURTHREAD] ← APPROACH:ACTPLACE[CELL:CAR[P]];
NewArray(REAL,TTHREAD:ANGLES[CURTHREAD],[LOJOINT:HIJOINT]);
TMPPLACE ← PPLANVAL(TTHREAD:PLACE[CURTHREAD],FLAG);
IF ¬FLAG THEN COMERR("Illegal approach point",MOV);
PLACESOL(TTHREAD:ANGLES[CURTHREAD],TMPPLACE,ARM,FLAG);
IF FLAG THEN
COMERR("This approach location is not accessible.
The closest reasonable point is being used.",TTHREAD:PLACE[CURTHREAD]);
TTHREAD:STIME[CURTHREAD] ←
RUNTIME(TTHREAD:ANGLES[OLDTHREAD],TTHREAD:ANGLES[CURTHREAD]);
TT ← TT + TTHREAD:STIME[CURTHREAD];
NewArray(REAL,TTHREAD:VELS[CURTHREAD],[LOJOINT:HIJOINT]);
NewArray(REAL,TTHREAD:COEFF[CURTHREAD],[LOJOINT:HIJOINT,0:5]);
TTHREAD:MODE[CURTHREAD] ← APPR_MODE;
SEGCNT ← SEGCNT + 1;
OLDTHREAD ← CURTHREAD;
CURTHREAD ← TTHREAD:NEXT[OLDTHREAD] ← NEW_RECORD(TTHREAD);
TTHREAD:PLACE[CURTHREAD] ← MOVE$:DEXP[MOV];
TTHREAD:MODE[CURTHREAD] ← ENDP_MODE + INVI_MODE;
NewArray(REAL,TTHREAD:ANGLES[CURTHREAD],[LOJOINT:HIJOINT]);
TMPPLACE ← PPLANVAL(TTHREAD:PLACE[CURTHREAD],FLAG);
IF ¬FLAG THEN COMERR("Illegal destination point",MOV);
PLACESOL(TTHREAD:ANGLES[CURTHREAD],TMPPLACE,ARM,FLAG);
IF FLAG THEN
COMERR("This destination location is not accessible.
The closest reasonable point is being used.",
CONS(MOV,CONS(TTHREAD:PLACE[CURTHREAD],RNULL)));
TTHREAD:STIME[CURTHREAD] ←
DEPTIME(TTHREAD:ANGLES[CURTHREAD],TTHREAD:ANGLES[OLDTHREAD],DEL);
TT ← TT + TTHREAD:STIME[CURTHREAD];
NewArray(REAL,TTHREAD:VELS[CURTHREAD],[LOJOINT:HIJOINT]);
NewArray(REAL,TTHREAD:COEFF[CURTHREAD],[LOJOINT:HIJOINT,0:5]);
FOR JOINT ← LOJOINT STEP 1 UNTIL HIJOINT DO
BEGIN ! stow away the coefficients for this joint. If
del is (final - initial), then the poly for each angle is
{del*t↑4 - 2*del*t↑3 + 2*del*t + initial}, but note that
DEL holds the negative of del at this point;
REAL DL;
DL ← - DEL[JOINT];
TTHREAD:COEFF[CURTHREAD][JOINT,4] ← DL;
TTHREAD:COEFF[CURTHREAD][JOINT,3] ← -2. * DL;
TTHREAD:COEFF[CURTHREAD][JOINT,1] ← 2. * DL;
TTHREAD:COEFF[CURTHREAD][JOINT,0] ←
TTHREAD:ANGLES[OLDTHREAD][JOINT];
TTHREAD:VELS[OLDTHREAD][JOINT] ←
2. * DL / TTHREAD:STIME[CURTHREAD];
END;
END "approa"
ELSE BEGIN "arrive"
! There is no deproach point, just put in the final point;
SEGCNT ← SEGCNT + 1;
OLDTHREAD ← CURTHREAD;
CURTHREAD ← TTHREAD:NEXT[OLDTHREAD] ← NEW_RECORD(TTHREAD);
TTHREAD:PLACE[CURTHREAD] ← MOVE$:DEXP[MOV];
TTHREAD:MODE[CURTHREAD] ← ENDP_MODE;
NewArray(REAL,TTHREAD:ANGLES[CURTHREAD],[LOJOINT:HIJOINT]);
NewArray(REAL,TTHREAD:VELS[CURTHREAD],[LOJOINT:HIJOINT]);
TMPPLACE ← PPLANVAL(TTHREAD:PLACE[CURTHREAD],FLAG);
IF ¬FLAG THEN COMERR("Illegal destination point",MOV);
PLACESOL(TTHREAD:ANGLES[CURTHREAD],TMPPLACE,ARM,FLAG);
IF FLAG THEN
COMERR("This destination location is not accessible.
The closest reasonable point is being used.",
CONS(MOV,CONS(TTHREAD:PLACE[CURTHREAD],RNULL)));
TTHREAD:STIME[CURTHREAD] ←
RUNTIME(TTHREAD:ANGLES[OLDTHREAD],TTHREAD:ANGLES[CURTHREAD]);
TT ← TT + TTHREAD:STIME[CURTHREAD];
NewArray(REAL,TTHREAD:COEFF[CURTHREAD],[LOJOINT:HIJOINT,0:5]);
END "arrive";
! See if there's an ON ARRIVAL clause;
P ← MOVE$:CLAUSES[MOV];
WHILE (P≠RNULL) ∧ ¬( (RECTYPE(Q←CELL:CAR[P])=LOC(CMON))
∧ (RECTYPE(Q←CMON:CONDITION[Q])=LOC(VARIABLE))
∧ EQU(".AE",VARIABLE:NAME[Q][1 FOR 3]) )
DO P ← CELL:CDR[P];
IF (P ≠ RNULL) THEN TTHREAD:EVENT[CURTHREAD] ← Q;
! Check for overall time constraints. Fulfil them if possible;
P ← MOVE$:CLAUSES[MOV];
WHILE (P≠RNULL) ∧ (RECTYPE(CELL:CAR[P])≠LOC(DURATION)) DO
P ← CELL:CDR[P];
IF P ≠ RNULL
THEN BEGIN "timcst"
! There is a global time constraint. Must try to fulfil it;
REAL AVAIL, CURTIM, FACTOR;
INTEGER M;
UT ← SVAL:VAL[SPLANVAL(DURATION:TIME[CELL:CAR[P]])];
IF UT>TT ∧ (DURATION:TIME_RELN[CELL:CAR[P]] LAND '1) ! (> or =);
THEN BEGIN "stretch" ! Easy case;
AVAIL ← 0.; ! Restricted extra time;
CURTIM ← 0; ! Currently used unrestricted time;
CURTHREAD ← TTHREAD:NEXT[MOTION]; ! First segment has no time;
WHILE CURTHREAD ≠ RNULL DO
BEGIN "timchk"
IF ¬((M←TTHREAD:MODE[CURTHREAD]) LAND INVI_MODE)
THEN ! Time in this segment not inviolate;
IF (M←M LAND '3) ≤ 1
THEN CURTIM ← CURTIM + TTHREAD:STIME[CURTHREAD]
ELSE IF M = 2
THEN AVAIL ← AVAIL + TTHREAD:UTIME[CURTHREAD]
- TTHREAD:STIME[CURTHREAD];
CURTHREAD ← TTHREAD:NEXT[CURTHREAD];
END "timchk";
IF CURTIM
THEN BEGIN "dostretch" ! Just modify those segments
which are not inviolate and have mode ≤ 1;
FACTOR ← (CURTIM + UT - TT) / CURTIM;
CURTHREAD ← TTHREAD:NEXT[MOTION];
WHILE CURTHREAD ≠ RNULL DO
BEGIN ! Expand right segments;
IF ¬((M←TTHREAD:MODE[CURTHREAD]) LAND INVI_MODE)
∧ (M LAND '3) ≤ 1
THEN TTHREAD:STIME[CURTHREAD]
← FACTOR * TTHREAD:STIME[CURTHREAD];
CURTHREAD ← TTHREAD:NEXT[CURTHREAD];
END;
END
ELSE COMERR(
"You want"&CVG(UT)&"seconds for this motion, and I could give you up
to"& CVG(TT+AVAIL) &", but I am only giving you"& CVG(TT)
&"instead.");
END "stretch"
ELSE IF UT<TT ∧ (DURATION:TIME_RELN[CELL:CAR[P]] LAND '3) ≥ 2 ! (< or =);
THEN BEGIN "shrink" ! Tough case;
COMERR(
"You want only" & CVG(UT) & "for this motion, and I think you need
" & CVG(TT) &". In order to satisfy your request, I am disregarding any
other time constraints you may have placed on the motion.");
CURTIM ← 0; ! Currently used non-inviolate time;
CURTHREAD ← TTHREAD:NEXT[MOTION]; ! First segment has no time;
WHILE CURTHREAD ≠ RNULL DO
BEGIN "timcnt"
IF ¬((M←TTHREAD:MODE[CURTHREAD]) LAND INVI_MODE)
THEN ! Time in this segment not inviolate;
CURTIM ← CURTIM + TTHREAD:STIME[CURTHREAD];
CURTHREAD ← TTHREAD:NEXT[CURTHREAD];
END "timcnt";
FACTOR ← (CURTIM + UT - TT) / CURTIM;
IF FACTOR ≤ 0
THEN BEGIN
COMERR(
"Your overall time constraint of" & CVG(UT) & "is ridiculous; I am
ignoring it.");
FACTOR ← 1.;
END;
CURTHREAD ← TTHREAD:NEXT[MOTION];
WHILE CURTHREAD ≠ RNULL DO
BEGIN ! Contract right segments;
IF ¬((M←TTHREAD:MODE[CURTHREAD]) LAND INVI_MODE)
THEN TTHREAD:STIME[CURTHREAD]
← FACTOR * TTHREAD:STIME[CURTHREAD];
CURTHREAD ← TTHREAD:NEXT[CURTHREAD];
END;
END "shrink";
END "timcst";
! Call the polynomial generator on chunks of the motion.
A chunk contains all segments between two velocity-constrained points;
OLDTHREAD ← MOTION;
CURTHREAD ← TTHREAD:NEXT[OLDTHREAD];
WHILE CURTHREAD ≠ RNULL DO
BEGIN "chunk"
! Each iteration finds one chunk, brackets it between
OLDTHREAD and CURTHREAD, and makes polys;
INTEGER PNTCNT; ! Counts number of points in each chunk;
PNTCNT ← 2; ! Count the end nodes this way;
WHILE (MEMLOC(TTHREAD:VELS[CURTHREAD],INTEGER) = 0) DO
BEGIN ! This chunk includes node pointed to by CURTHREAD;
PNTCNT ← PNTCNT + 1;
CURTHREAD ← TTHREAD:NEXT[CURTHREAD];
END;
! Now OLDTHREAD and CURTHREAD point to nodes on each end of
chunk;
IF PNTCNT = 2
THEN IF TTHREAD:MODE[OLDTHREAD]=APPR_MODE ∨
TTHREAD:MODE[CURTHREAD]=DEPA_MODE
THEN ! This is an approach or departure segment, so
the polynomials have already been calculated;
ELSE
! Two-point system. Use the fifth-order polynomial
which will meet the position, velocity, (and
acceleration) constraints. This is faster than the
older version which inserted two equispaced
unconstrained points and solved third degree
polynomials using POLY;
FOR JOINT ← LOJOINT STEP 1 UNTIL HIJOINT DO
BEGIN "oneseg"
REAL P0, V0, A0, P1, V1, A1;
! Initial, final position, vel, acc;
OWN REAL ARRAY COE[0:5]; ! For efficiency;
ST ← TTHREAD:STIME[CURTHREAD];
P0 ← TTHREAD:ANGLES[OLDTHREAD][JOINT];
P1 ← TTHREAD:ANGLES[CURTHREAD][JOINT];
V0 ← TTHREAD:VELS[OLDTHREAD][JOINT]*ST;
V1 ← TTHREAD:VELS[CURTHREAD][JOINT]*ST;
A0 ← 0.0; ! *ST*ST;
A1 ← 0.0; ! *ST*ST;
COE[0] ← P0;
COE[1] ← V0;
COE[2] ← A0 / 2.;
COE[3] ← -(8.*V1 + 12.*V0 + 3.*A0 - A1 - 20.*P1 + 20.*P0) / 2.;
COE[4] ← (14.*V1 + 16.*V0 + 3.*A0 - 2.*A1 - 30.*P1 + 30.*P0) / 2.;
COE[5] ← -(6.*V1 + 6.*V0 + A0 - A1 - 12.*P1 + 12.*P0) / 2.;
ARRBLT(TTHREAD:COEFF[CURTHREAD][JOINT,0],COE[0],6);
END "oneseg"
ELSE BEGIN "sevseg" ! There are several segments. Find
the two with longest intervals, and put free
points there. There is some dispute as to the
best place to put these points. The current
version puts them very close to the beginning of
the interval. An older version put them in the
middle, and this led to massive overshoots;
REAL T1, T2; ! Longest, next longest times;
RPTR(TTHREAD) Q1, Q2; ! Longest, next longest segment starters;
T1 ← T2 ← 0.;
Q ← OLDTHREAD;
P ← TTHREAD:NEXT[Q];
WHILE Q ≠ CURTHREAD DO
BEGIN "max" ! This loop finds the two longest intervals;
IF (ST←TTHREAD:STIME[P]) > T2
THEN IF ST > T1
THEN BEGIN ! New longest;
T2 ← T1;
T1 ← ST;
Q2 ← Q1;
Q1 ← Q;
END
ELSE BEGIN ! New next-longest;
T2 ← ST;
Q2 ← Q;
END;
Q ← P;
P ← TTHREAD:NEXT[P]
END "max";
P ← TTHREAD:NEXT[Q1];
Q ← TTHREAD:NEXT[Q1] ← NEW_RECORD(TTHREAD);
TTHREAD:NEXT[Q] ← P;
TTHREAD:STIME[Q] ← T1 * 0.001;
TTHREAD:STIME[P] ← T1 - TTHREAD:STIME[Q];
NewArray(REAL,TTHREAD:COEFF[Q],[LOJOINT:HIJOINT,0:5]);
P ← TTHREAD:NEXT[Q2];
Q ← TTHREAD:NEXT[Q2] ← NEW_RECORD(TTHREAD);
TTHREAD:NEXT[Q] ← P;
TTHREAD:STIME[Q] ← T2 * 0.001;
TTHREAD:STIME[P] ← T2 - TTHREAD:STIME[Q];
NewArray(REAL,TTHREAD:COEFF[Q],[LOJOINT:HIJOINT,0:5]);
POLY(OLDTHREAD,CURTHREAD,LOJOINT,HIJOINT,PNTCNT+1);
! FOR JOINT ← LOJOINT STEP 1 UNTIL HIJOINT DO
POLY(OLDTHREAD,CURTHREAD,JOINT,PNTCNT+1);
END "sevseg";
OLDTHREAD ← CURTHREAD;
CURTHREAD ← TTHREAD:NEXT[OLDTHREAD];
END "chunk";
! Compute the gravity and inertia terms;
CURTHREAD ← TTHREAD:NEXT[MOTION];
WHILE CURTHREAD ≠ RNULL DO
BEGIN "grav"
REQUIRE "BEJCZY.REL[AL,HE]" LOAD_MODULE;
REQUIRE "FAITRG.REL[AL,HE]" LOAD_MODULE;
EXTERNAL PROCEDURE DTERMS(REFERENCE REAL RES, ANG; INTEGER ARM);
IF MEMLOC(TTHREAD:ANGLES[CURTHREAD],INTEGER) = 0
THEN BEGIN ! Need to set up angles array on this free point;
NewArray(REAL,TTHREAD:ANGLES[CURTHREAD],[LOJOINT:HIJOINT]);
FOR JOINT ← LOJOINT STEP 1 UNTIL HIJOINT DO
TTHREAD:ANGLES[CURTHREAD][JOINT] ← TTHREAD:COEFF[CURTHREAD][JOINT,0];
END;
NewArray(REAL,TTHREAD:GRAVIN[CURTHREAD],[2*LOJOINT:2*HIJOINT+1]);
DTERMS(TTHREAD:GRAVIN[CURTHREAD][2*LOJOINT],
TTHREAD:ANGLES[CURTHREAD][LOJOINT], ARM);
CURTHREAD ← TTHREAD:NEXT[CURTHREAD];
END "grav";
! Output the motion table;
! The format for the coefficients is described in ARM.PAL[11,BES].
SERVO BIT WORD
YARMSB, YHANDSB, BARMSB, or BHANDSB
SERVO BIT WORD
COMMAND BITS; DEFINE NONULLCB = "'1"; ! No end null;
DEFINE WOBBLECB = "'2"; ! Wobble at end;
DEFINE DEPARTCB = "'4"; ! ∃ Departure point;
DEFINE RTMOVECB = "'10"; ! Experimental move
WOBBLE VALUE POINTER Pointer to wobble value for this motion (constant)
RELATIVE SEGMENT PTR Length (bytes) of first segment table
8 + 32*njoints. 0 means there are no more
segments. (Put at very end of table)
TIME milliseconds for this segment
TRANS ptr to list of transforms+valid. nos:
L-O Level-offset of first trans (or scalar)
VAL Place for validity number
MECH Mechanism number:
YARM_MECH, YHAND_MECH, BARM_MECH or BHAND_MECH
L-O Level-offset for next variable
:
MECH Last mechanism number
CODE ptr to code to be scheduled at end
of this segment
A0 coeff (floating) first joint
:
A5 last coeff, first joint
A0 first coeff, second jolint
:
:
:
A5 last coeff, last joint
NCI final joint gravity loading, first joint
NCII final joint inertia loading, first joint
:
:
NCI final joint gravity loading, last joint
NCII final joint inertia loading, last joint
RELATIVE SEGMENT PTR
This is the format for the transform-validity list:
T1 level-offset of transform for first arm
0 room for validity number
:
Tn level-offset of transform for last arm
0 room for validity number
DATA and RELOC are used to output the motion table. Each of
these is 1000 long (very long motions may not fit.). See
EMITER.HDR for the pseudo-op definitions.
;
ARRCLR(RELOC);
ARRCLR(DATA);
! Output pseudo-op for motion;
LAB ← GENLABEL; ! Points to motion table;
EMIT(PSDCODE,MOVE_PSOP,PSINST);
EMIT(PSDCODE,LAB,SYMREF);
EMIT(PSDCODE,ARM,CONST);
! EMIT(PSDCODE,ERROR_BITS,CONST) - in PASS3 - bit mask for error handler;
! EMIT(PSDCODE,LAB2,SYMREF) - in PASS3 - tells us where to go for next pcode;
! EMIT(PSDCODE,LAB1,SYMREF) - in PASS3 - tells us where to go for a retry;
! Output trajectory file;
MAKE_REMARK(TJFILE,"Motion table");
EMIT(TJFILE,LAB,SYMDEC);
! Check for nulling; ! No_nulling is once again the default;
P ← MOVE$:CLAUSES[MOV];
WHILE (P≠RNULL) ∧ (RECTYPE(CELL:CAR[P])≠LOC(NNULL)) DO P ← CELL:CDR[P];
IF (P=RNULL) ∨ (NNULL:FLAG[CELL:CAR[P]]) THEN DATA[6] ← NONULLCB;
! Check for run-time move;
P ← MOVE$:CLAUSES[MOV];
WHILE (P≠RNULL) ∧ (RECTYPE(CELL:CAR[P])≠LOC(RTMOVE)) DO P ← CELL:CDR[P];
IF (P≠RNULL) THEN DATA[6] ← DATA[6] LOR RTMOVECB;
! Check for wobble;
P ← MOVE$:CLAUSES[MOV];
WHILE (P≠RNULL) ∧ (RECTYPE(CELL:CAR[P])≠LOC(WOBBLE)) DO P ← CELL:CDR[P];
DATA[4] ← SBITS; ! servo bits;
RELOC[4] ← CONST;
DATA[5] ← 0; ! No second servo bits;
DATA[6] ← ! want DEPARTCB only if this motion has a deproach;
DATA[6] LOR ! this already contains the desired nulling bit;
(IF TTHREAD:MODE[TTHREAD:NEXT[MOTION]] LAND DEPA_MODE THEN DEPARTCB ELSE 0)
LOR (IF P≠RNULL THEN WOBBLECB ELSE 0);
RELOC[5] ← RELOC[6] ← CONST;
EMIT(TJFILE,DATA[4],RELOC[4],3); ! Label, servo bits, servo bits, command bits;
IF P≠RNULL THEN ! Emit pointer to wobble value & the actual value;
BEGIN
EMIT(TJFILE,LAB←GENLABEL,SYMREF,1); ! Point to the wobble value;
MAKE_REMARK(SMLBLK,"Wobble value for motion");
EMIT(SMLBLK,LAB,SYMDEC,1); ! Here is the wobble value;
DATA[7] ← MEMORY[LOC(SVAL:VAL[WOBBLE:VAL[CELL:CAR[P]]]),INTEGER];
RELOC[7] ← FLOAT;
EMIT(SMLBLK,DATA[7],RELOC[7],1);
END
ELSE EMIT(TJFILE,0,CONST,1); ! The absence of a wobble value;
SEGLEN ← 8 + 32*(IF (ARM = YARM_MECH) OR (ARM = BARM_MECH) THEN 6 ELSE 1);
DATA[7] ← SEGLEN; RELOC[7] ← CONST;
Q ← TTHREAD:NEXT[MOTION];
WHILE Q ≠ RNULL DO
BEGIN "coeout" ! Coefficients for one segment;
INTEGER PLACETYPE; ! One of EXPRN, VALU$, VARIABLE;
P ← Q;
Q ← TTHREAD:NEXT[Q];
IF TTHREAD:PLACE[P] = RNULL THEN CONTINUE "coeout";
! Avoid outputting the short segments that end at
unconstrained points;
MAKE_REMARK(TJFILE,"Relative segment pointer");
DATA[8] ← TTHREAD:STIME[P]*1000.; RELOC[8] ← CONST; ! Milliseconds;
EMIT(TJFILE,DATA[7],RELOC[7],2); ! Relative seg ptr, time;
PLACETYPE ← RECTYPE(DEXPR:EXPN[TTHREAD:PLACE[P]]);
IF PLACETYPE=LOC(VARIABLE) OR PLACETYPE=LOC(EXPRN)
THEN BEGIN "needtrans"
! There is an associated place, need a trans pointer;
EMIT(TJFILE,LAB←GENLABEL,SYMREF,1); ! Refer to the trans pointer;
MAKE_REMARK(SMLBLK,"Trans pointer for motion");
EMIT(SMLBLK,LAB,SYMDEC,1); ! Here is the trans pointer;
EMIT(SMLBLK,VARIABLE:OFFSET[DEXPR:VAR[TTHREAD:PLACE[P]]],CONST,1);
! Point to the trans itself;
! Currently only one frame allowed;
EMIT(SMLBLK,0,CONST,1); ! Leave room for the validity bit;
END "needtrans"
ELSE EMIT(TJFILE,0,CONST,1); ! The absence of a trans pointer;
IF TTHREAD:EVENT[P]≠RNULL
THEN BEGIN ! There is some associated code, need to signal to it.;
EMIT(TJFILE,LAB←GENLABEL,SYMREF,1); ! Refer to the event pointer;
MAKE_REMARK(SMLBLK,"Event pointer for VIA code");
EMIT(SMLBLK,LAB,SYMDEC,1); ! Here is the event pointer;
EMIT(SMLBLK,VARIABLE:OFFSET[TTHREAD:EVENT[P]],CONST,1);
! Point to the event itself;
EMIT(SMLBLK,0,CONST,1); ! Leave room for the armcode to use;
END
ELSE
EMIT(TJFILE,0,CONST,1); ! The absence of associated code;
DPTR ← 9;
MAKE_REMARK(TJFILE,"Coefficients, gravity, inertia");
! Coefficients;
FOR JOINT← LOJOINT STEP 1 UNTIL HIJOINT DO
BEGIN ! Each iteration spits out the coefficient of one joint;
INTEGER DEGR;
FOR DEGR ← 0 STEP 1 UNTIL 5 DO
DATA[DPTR+DEGR] ← MEMORY[LOC(TTHREAD:COEFF[P][JOINT,DEGR]),INTEGER];
DPTR ← DPTR + 6;
END;
! The gravity and inertia terms;
FOR I ← 2*LOJOINT STEP 1 UNTIL 2*HIJOINT+1 DO
BEGIN ! Each 2 iterations spits out the terms for one joint;
DATA[DPTR] ← MEMORY[LOC(TTHREAD:GRAVIN[P][I]),INTEGER];
DPTR ← DPTR+1;
END;
RELOC[9] ← FLOAT; ! They are all floating point constants;
ARRBLT(RELOC[10],RELOC[9],DPTR-10);
EMIT(TJFILE,DATA[9],RELOC[9],DPTR-9); ! All the coefficients for this seg;
END "coeout";
EMIT(TJFILE,0,CONST,1); ! The last relative segment pointer is 0;
MAKE_REMARK(TJFILE,"End of motion table");
! Reclaim all the arrays in the motion thread;
Q ← MOTION;
WHILE Q ≠ RNULL DO
BEGIN "reclaim"
INTEGER ADR;
EXTERNAL PROCEDURE ARYEL (INTEGER ADRESS); ! In the SAIL segment;
ADR ← 0;
MEMLOC(TTHREAD:ANGLES[Q],INTEGER) ↔ ADR;
IF ADR THEN ARYEL(ADR);
ADR ← 0;
MEMLOC(TTHREAD:VELS[Q],INTEGER) ↔ ADR;
IF ADR THEN ARYEL(ADR);
ADR ← 0;
MEMLOC(TTHREAD:COEFF[Q],INTEGER) ↔ ADR;
IF ADR THEN ARYEL(ADR);
ADR ← 0;
MEMLOC(TTHREAD:GRAVIN[Q],INTEGER) ↔ ADR;
IF ADR THEN ARYEL(ADR);
Q ← TTHREAD:NEXT[Q];
END "reclaim";
! End of TRJCLC;
END "trjclc";
! CENTCLC, STOPCLC;
INTERNAL PROCEDURE CENTCLC (RPTR(CENTER) CNTR);
BEGIN "centclc"
! The "trajectory" table looks like this:
COFLST: XXXXXX TWO SERVO BIT WORDS, 7 BITS MUST BE ON, A HAND
XXXXXX SERVO AND ALL JOINT SERVOS OF THE SAME ARM
0 NO COMMAND BITS
0 NO WOBBLE VALUE
0 NO NEXT SEGMENT
0 NO FUNCTION TIME
0 NO TRANSFORM
CODE PTR TO CODE TO BE SCHEDULED THIS SEG
NO POLYNOMIAL TO FOLLOW
;
! Does not yet handle any cmons or code;
INTEGER ARM, SBITS, LAB;
INTEGER LOJOINT, HIJOINT; ! Not used;
PRELOAD_WITH 0,0,0,0,0,0;
OWN INTEGER ARRAY ZEROS[1:6];
PRELOAD_WITH CONST,CONST,CONST,CONST,CONST,CONST;
OWN INTEGER ARRAY CONSTS[1:6];
DEVBITS(ARM,SBITS,LOJOINT,HIJOINT,CENTER:CF[CNTR]);
! Want to turn on the hand bits as well:
IF ARM = YARM_MECH
THEN BEGIN
ARM ← YARM_MECH + YHAND_MECH
SBITS ← YARMSB + YHANDSB
END
ELSE IF ARM = BARM_MECH
THEN BEGIN
ARM ← BARM_MECH + BHAND_MECH
SBITS ← BARMSB + BHANDSB
END;
! Do it like this:;
ARM ← ARM LOR (ARM LSH 1);
SBITS ← SBITS LOR (SBITS LSH -1);
EMIT(PSDCODE,CENTER_PSOP,PSINST);
LAB ← GENLABEL;
EMIT(PSDCODE,LAB,SYMREF);
EMIT(PSDCODE,ARM,CONST);
! EMIT(PSDCODE,LAB1,SYMREF) - in PASS3 - tells us where to go for a retry;
MAKE_REMARK(TJFILE,"Center table");
EMIT(TJFILE,LAB,SYMDEC);
EMIT(TJFILE,SBITS,CONST);
EMIT(TJFILE,ZEROS[1],CONSTS[1],6);
END "centclc";
INTERNAL PROCEDURE STOPCLC(RPTR(STOP) STP);
BEGIN "stopclc"
INTEGER ARM, SBITS, LOJOINT, HIJOINT; ! Only ARM is used;
DEVBITS(ARM,SBITS,LOJOINT, HIJOINT, STOP:CF[STP]);
EMIT(PSDCODE,STOP_PSOP,PSINST);
EMIT(PSDCODE,ARM,CONST);
END "stopclc";
END $$prgid;
! Bugs
;